package robot;

import java.awt.geom.AffineTransform;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import javaclient3.FiducialInterface;
import javaclient3.PlayerClient;
import javaclient3.PlayerException;
import javaclient3.Position2DInterface;
import javaclient3.structures.PlayerConstants;
import javaclient3.structures.PlayerPose3d;
//import javaclient3.structures.fiducial.PlayerFiducialData;
import javaclient3.structures.fiducial.PlayerFiducialData;
import javaclient3.structures.fiducial.PlayerFiducialItem;

public class Robot extends Thread {
	private static double ROTATION_SPEED = 1;
	private static double MAX_SPEED = 0.5;

	public static double MAX_DIST_GOAL = 0.1;
	
	PlayerClient client;
	Position2DInterface pos;
	FiducialInterface fiducial;
	ArrayList<PathPoint> pathpoints;
	//ArrayList<MaterialObject> detectedObjects;
	private long startTime;
	
	/**
	 * 
	 */
	public Robot() {
		try {
			connectPlayer();
			pos = client.requestInterfacePosition2D (0, PlayerConstants.PLAYER_OPEN_MODE);
			fiducial = client.requestInterfaceFiducial(0, PlayerConstants.PLAYER_OPEN_MODE);
			//PlannerInterface plni = robot.requestInterfacePlanner    (0, PlayerConstants.PLAYER_OPEN_MODE);
		}
        catch (PlayerException e) {
            System.err.println("NavigatorExample: Error connecting to Player: ");
            System.err.println("    [ " + e.toString() + " ]");
            System.exit(1);
        }
        
		waitForFiducial();
		
		pos.setMotorPower(PlayerConstants.PLAYER_POSITION3D_MOTOR_POWER);
	}
	
	
	
	public void run() {
		startTime = System.currentTimeMillis();
		int indexPoint = 0;
		while(true) {
			client.readAll();
			
			if (indexPoint < pathpoints.size()) {
				if (goToStrainght(pathpoints.get(indexPoint))) {
					indexPoint++;
					System.out.println("Waypoint " + indexPoint + " reached");
				}
			} else {
				pos.setSpeed(0, 0);
			}

			//detectedObjects = detectObjects();
			
			try { Thread.sleep (10); } catch (Exception e) { e.printStackTrace(); }
		}
	}
	
	protected void finalize() throws Throwable {
		closePlayer();
	}
	
	private void connectPlayer() {
		client = new PlayerClient("localhost",6665);
	}
	
	private void closePlayer() {
		client.close();
	}
	
	private void waitForFiducial() {
    	while(!fiducial.isDataReady()) {
    		client.readAll();
    	}
	}
	
	/**
	 * @return the pos
	 */
	public Position2DInterface getPos() {
		return pos;
	}

	/**
	 * @return the fiducial
	 */
	public FiducialInterface getFiducial() {
		return fiducial;
	}
	
	public ArrayList<PathPoint> getPathpoints() {
		return pathpoints;
	}

	public void setPathpoints(ArrayList<PathPoint> arrayList) {
		this.pathpoints = arrayList;
	}
	
	public boolean goToStrainght(PathPoint goal) {
		PlayerPose3d goalPose = goal.getPosition();
		double distance = Point2D.distance(pos.getX(), pos.getY(), goalPose.getPx(), goalPose.getPy());
		if (distance <= MAX_DIST_GOAL) {
			return true;
		} else {
			double angleDiff = (Math.atan2(goalPose.getPy() - pos.getY(), goalPose.getPx() - pos.getX()) - pos.getYaw()) % Math.PI;
			double turnrate;
			if (Math.abs(angleDiff * 0.1) < Math.abs(Math.signum(angleDiff) * ROTATION_SPEED)) {
				turnrate = angleDiff * 2;
			} else {
				turnrate = Math.signum(angleDiff) * ROTATION_SPEED;
			}
			double speed;

			if (Math.abs(angleDiff) < Math.PI/2) {
				speed = Math.min(Math.cos(angleDiff) * distance * 1000 / Math.max(startTime + goal.getTime() - System.currentTimeMillis(), 0.1), MAX_SPEED);
			} else {
				speed = 0;
			}
			
			pos.setSpeed(speed, turnrate);
			return false;
		}
	}
	
	public PlayerPose3d getPosition() {
		PlayerPose3d position = new PlayerPose3d();
		position.setPx(pos.getX());
		position.setPy(pos.getY());
		position.setPyaw(pos.getYaw());
		return position;
	}

	private AffineTransform getTransfo(PlayerPose3d positionRobot) {
		//Graphics2D ;
		AffineTransform transfo = new AffineTransform();
		transfo.translate(positionRobot.getPx(), positionRobot.getPy());
		transfo.rotate(positionRobot.getPyaw());
		//transfo.scale(sx,sy);
		return transfo;
	}
	
	private PlayerPose3d getAbsPos(PlayerPose3d relPose, AffineTransform transfo) {
		Point2D.Double relPosp2d = new Point2D.Double(relPose.getPx(), relPose.getPy());
		Point2D.Double absPosp2d = (Point2D.Double) relPosp2d.clone();
		PlayerPose3d absPose = new PlayerPose3d();
		double angleRobot = pos.getYaw();
		relPosp2d.setLocation(relPose.getPx(), relPose.getPy());
		
		transfo.transform(relPosp2d, absPosp2d);
		//System.out.println("in: " + relPosp2d.toString() + ", out: " + absPosp2d.toString());

		absPose.setPx(absPosp2d.getX());
		absPose.setPy(absPosp2d.getY());
		absPose.setPyaw(relPose.getPyaw() + angleRobot);
		return absPose;
	}

	public ArrayList<MaterialObject> detectObjects() {
		
		PlayerFiducialData fiducialdata = fiducial.getData();
		PlayerPose3d positionRobot = getPosition();
		PlayerFiducialItem[] fiducials = fiducialdata.getFiducials();
		//System.out.println(fiducials.length + " Objects");
		ArrayList<MaterialObject> detectedObjects = new ArrayList<MaterialObject>();
		
		AffineTransform transformation = getTransfo(positionRobot);

		for (int ifid = 0; ifid < fiducials.length; ifid++) {  // i indexes each element successively.
			MaterialObject detectedObject = new MaterialObject();
			detectedObject.setId(fiducials[ifid].getId());
			detectedObject.setPose(fiducials[ifid].getPose());
			detectedObject.setUpose(fiducials[ifid].getUpose());
			detectedObject.setAbsPose(getAbsPos(fiducials[ifid].getPose(), transformation));
			
			detectedObjects.add(detectedObject);
		}
		
		return detectedObjects;
	}	
}
